/**
 * @(#) NXTTiltSensor.java
 */

package pcsr.drivers.nxtDriver;

import pcsr.drivers.nxtDriver.command.INXTCommand;

class NXTTiltSensor extends NXTSensor
{
	private static byte X_TILT = 0x42;
	private static byte Y_TILT = 0x43;
	private static byte Z_TILT = 0x44;
	
	private static byte X_ACCEL_LSB = 0x45;
	private static byte Y_ACCEL_LSB = 0x47;
	private static byte Z_ACCEL_LSB = 0x49;
	
	NXTTiltSensor( NXTPort sensor, INXTCommand command ) {
		super(sensor, command);
		setTypeAndMode(NXTSensorType.LOWSPEED9V, NXTSensorMode.RAW);
	}

	/**
	 * Tilt of sensor along X-axis (see top of Mindsensors.com sensor for
	 * diagram of axis).  128 is level.
	 * @return value of tilt in the Xs axes
	 */
	public int getXTilt( ) {
		byte [] val = getData(X_TILT, 1);
		return 0xFF & val[0];
	}
	
	public int getYTilt( ) {
		byte [] val = getData(Y_TILT, 1);
		return 0xFF & val[0];
	}
	
	public int getZTilt( ) {
		byte [] val = getData(Z_TILT, 1);
		return 0xFF & val[0];
	}
	
	/**
	 * Acceleration along X axis. Positive or negative values in mg.
	 * (g = acceleration due to gravity = 9.81 m/s^2)
	 * @return Acceleration e.g. 9810 mg (falling on earth)
	 */
	public int getXAccel( ) {
		byte [] vals = getData(X_ACCEL_LSB, 2);
		//System.out.print("LSB = " + vals[0] + "  MSB = " + vals[1] + "   ");
		int accel = (vals[0]) | ((vals[1]) << 8);
		return accel;
	}
	
	public int getYAccel( ) {
		byte [] vals = getData(Y_ACCEL_LSB, 2);
		//System.out.print("LSB = " + vals[0] + "  MSB = " + vals[1] + "   ");
		int accel = (vals[0]) | ((vals[1]) << 8);
		return accel;
	}
	
	public int getZAccel( ) {
		byte [] vals = getData(Z_ACCEL_LSB, 2);
		//System.out.print("LSB = " + vals[0] + "  MSB = " + vals[1] + "   ");
		int accel = (vals[0]) | ((vals[1]) << 8);
		return accel;
	}

	@Override
	public double GetSensorValue( ) {
		//return new byte[]{X_TILT, Y_TILT, Z_TILT, X_ACCEL_LSB, Y_ACCEL_LSB, Z_ACCEL_LSB};
		return getXTilt() | getYTilt() | getZTilt() | getXAccel() | getYAccel() | getZAccel();
	}

	@Override
	public NXTSensorMode[] GetAvailableModes( ) {
		return new NXTSensorMode[]{NXTSensorMode.RAW};
	}

	@Override
	int UpdateActuatorState( ) {
		SetSensorValue(GetSensorValue());
		return 0;
	}
}
